//!Includes

#include "TrackingTask.h"

//!Global Functions


namespace tasks
{

/**
 * class TrackingTask
 *
 *
 * Autor:David Bevilaqua
 *
 *
 * DESCRIPTION:
 *
 */

//!Constructors/Destructors

/**
 * Empty Constructor
 */
TrackingTask::TrackingTask ( )
{
	initAttributes ( );
}

/**
 * Empty Destructor
 */
TrackingTask::~TrackingTask ( )
{

}

//!Public Accessor Methods

ArClientBase *TrackingTask::GetArClient()
{
	return m_pClient;
}

void TrackingTask::SetArClient(ArClientBase *client)
{
	m_pClient=client;
	m_pOutput=new algorithms::OutputHandler(m_pClient);
}

//!Public Methods

void TrackingTask::ShowSourceImage(int t,std::string string)
{
	cvShowImage(string.c_str(), m_pSourceImage );
	cvWaitKey(t);
	string.clear();

}

void TrackingTask::ShowDestinImage(int t,std::string string)
{
	cvShowImage(string.c_str(), m_pDestinImage );
	cvWaitKey(t);
	string.clear();
}

void TrackingTask::ShowRoiImage(int t,std::string string)
{
	cvShowImage(string.c_str(), m_pRoi );
	cvWaitKey(t);
	string.clear();
}

void TrackingTask::ReleaseSourceImage( )
{
	cvReleaseImage(&m_pSourceImage);
}

void TrackingTask::ReleaseDestinImage( )
{
	cvReleaseImage(&m_pDestinImage);
}

void TrackingTask::ReleaseRoiImage( )
{
	cvReleaseImage(& m_pRoi);
}

void TrackingTask::FindTarget( )
{
	m_Segmentation.SetDestinImage(m_pDestinImage);
	m_Segmentation.SetSourceImage(m_pSourceImage);
	m_Segmentation.SetTarget(&m_TargetBox);
	m_bFindTarget=m_Segmentation.FindTarget();

	if(m_bFindTarget)
	{
		m_TargetBox=m_Segmentation.GetTarget();
		SetRoi(cvPoint(m_TargetBox.center.x-m_TargetBox.size.width/2,m_TargetBox.center.y-m_TargetBox.size.height/2),cvPoint(m_TargetBox.center.x+m_TargetBox.size.width/2,m_TargetBox.center.y+m_TargetBox.size.height/2));
	}

	m_pDestinImage=m_Segmentation.GetDestinImage();

	ReleaseDestinImage();


}

void TrackingTask::FindTargetPoints( )
{
	m_Segmentation.SetDestinImage(m_pDestinImage);
	m_Segmentation.SetSourceImage(m_pRoi);
	if(m_bFindTarget)
	{
		m_bFindTargetPoints=m_Segmentation.FindTargetPoints();
		m_pDestinImage=m_Segmentation.GetDestinImage();
		if(m_bFindTargetPoints)
		{
			m_TargetPointsTemp=m_Segmentation.GetTargetPoints();
			//m_TargetPointsTemp.clear();
			//m_Segmentation.ClearTargetPoints();
			m_bNewVect=true;
		}
		else
		{
			m_bNewVect=false;
		}
	}
	else
	{
		m_bNewVect=false;
	}
}

void TrackingTask::FillTarget(int val )
{
if(val==IBVS)
{
	if(m_bFindTargetPoints&&m_bFindTarget)
	{
		algorithms::Target Targetemp;
		if(m_iCountTarget<=m_iCountMaxTarget)
		{
			m_bFillVect=false;
			Targetemp.m_TargetPoints[0].x=m_TargetPointsTemp.at(0).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[1].x=m_TargetPointsTemp.at(1).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[2].x=m_TargetPointsTemp.at(2).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[3].x=m_TargetPointsTemp.at(3).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[4].x=m_TargetPointsTemp.at(4).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[0].y=m_TargetPointsTemp.at(0).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPoints[1].y=m_TargetPointsTemp.at(1).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPoints[2].y=m_TargetPointsTemp.at(2).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPoints[3].y=m_TargetPointsTemp.at(3).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPoints[4].y=m_TargetPointsTemp.at(4).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPointsRef[0].x=m_Reference.m_TargetPoints[0].x;
			Targetemp.m_TargetPointsRef[1].x=m_Reference.m_TargetPoints[1].x;
			Targetemp.m_TargetPointsRef[2].x=m_Reference.m_TargetPoints[2].x;
			Targetemp.m_TargetPointsRef[3].x=m_Reference.m_TargetPoints[3].x;
			Targetemp.m_TargetPointsRef[4].x=m_Reference.m_TargetPoints[4].x;
			Targetemp.m_TargetPointsRef[0].y=m_Reference.m_TargetPoints[0].y;
			Targetemp.m_TargetPointsRef[1].y=m_Reference.m_TargetPoints[1].y;
			Targetemp.m_TargetPointsRef[2].y=m_Reference.m_TargetPoints[2].y;
			Targetemp.m_TargetPointsRef[3].y=m_Reference.m_TargetPoints[3].y;
			Targetemp.m_TargetPointsRef[4].y=m_Reference.m_TargetPoints[4].y;
			Targetemp.SetPoseRobot(cvPoint((int)m_pOutput->myX,(int)m_pOutput->myY),m_pOutput->myTh);
			Targetemp.SartUp();
			m_TargetVector.push_back(Targetemp);
			m_iCountTarget++;

		}
		else
		{
			m_bFillVect=true;
			Targetemp.m_TargetPoints[0].x=m_TargetPointsTemp.at(0).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[1].x=m_TargetPointsTemp.at(1).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[2].x=m_TargetPointsTemp.at(2).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[3].x=m_TargetPointsTemp.at(3).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[4].x=m_TargetPointsTemp.at(4).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			Targetemp.m_TargetPoints[0].y=m_TargetPointsTemp.at(0).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPoints[1].y=m_TargetPointsTemp.at(1).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPoints[2].y=m_TargetPointsTemp.at(2).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPoints[3].y=m_TargetPointsTemp.at(3).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPoints[4].y=m_TargetPointsTemp.at(4).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			Targetemp.m_TargetPointsRef[0].x=m_Reference.m_TargetPoints[0].x;
			Targetemp.m_TargetPointsRef[1].x=m_Reference.m_TargetPoints[1].x;
			Targetemp.m_TargetPointsRef[2].x=m_Reference.m_TargetPoints[2].x;
			Targetemp.m_TargetPointsRef[3].x=m_Reference.m_TargetPoints[3].x;
			Targetemp.m_TargetPointsRef[4].x=m_Reference.m_TargetPoints[4].x;
			Targetemp.m_TargetPointsRef[0].y=m_Reference.m_TargetPoints[0].y;
			Targetemp.m_TargetPointsRef[1].y=m_Reference.m_TargetPoints[1].y;
			Targetemp.m_TargetPointsRef[2].y=m_Reference.m_TargetPoints[2].y;
			Targetemp.m_TargetPointsRef[3].y=m_Reference.m_TargetPoints[3].y;
			Targetemp.m_TargetPointsRef[4].y=m_Reference.m_TargetPoints[4].y;
			m_TargetVector.erase(m_TargetVector.begin());
			Targetemp.SetPoseRobot(cvPoint((int)m_pOutput->myX,(int)m_pOutput->myY),m_pOutput->myTh);
			Targetemp.SartUp();
			m_TargetVector.push_back(Targetemp);


		}

		cvCircle(m_pSourceImage,Targetemp.m_TargetPoints[0],5,cvScalar(0,200,200),-1);
		cvCircle(m_pSourceImage,Targetemp.m_TargetPoints[1],5,cvScalar(0,200,200),-1);
		cvCircle(m_pSourceImage,Targetemp.m_TargetPoints[2],5,cvScalar(0,200,200),-1);
		cvCircle(m_pSourceImage,Targetemp.m_TargetPoints[3],5,cvScalar(0,200,200),-1);
		cvCircle(m_pSourceImage,Targetemp.m_TargetPoints[4],5,cvScalar(0,200,200),-1);
		cvCircle(m_pSourceImage,Targetemp.GetCenter(),5,cvScalar(200,200,200),-1);

		cvLine(m_pSourceImage,Targetemp.m_TargetPoints[0],Targetemp.m_TargetPoints[2],cvScalar(200,0,0));
		cvLine(m_pSourceImage,Targetemp.m_TargetPoints[1],Targetemp.m_TargetPoints[3],cvScalar(200,0,0));
		m_TargetPointsTemp.clear();

		m_Segmentation.ClearTargetPoints();
	}
	else if(m_bFillVect==true)
	{
		//printf("1:%d\n",m_TargetVector.at(0).GetDiffCenterX());
		//printf("2:%d\n",m_TargetVector.at(1).GetDiffCenterX());
		//printf("3:%d\n",m_TargetVector.at(2).GetDiffCenterX());
		//printf("size:%d\n",m_TargetVector.size());
		m_TargetVector.back().SetPoseRobot(cvPoint((int)m_pOutput->myX,(int)m_pOutput->myY),m_pOutput->myTh);
	}

}
else if(val==PBVS)
{
	if(m_iCountTarget==0)
	{
		m_bFillVect=true;
		algorithms::Target Targetemp;
		Targetemp.SetPoseRobot(cvPoint((int)m_pOutput->myX,(int)m_pOutput->myY),m_pOutput->myTh);
		Targetemp.SetPoseTarget(cvPoint(4000,4000),30);
		Targetemp.SartUp();
		m_TargetVector.push_back(Targetemp);
		m_iCountTarget++;
	}
	else
	{
		m_TargetVector.erase(m_TargetVector.begin());
		algorithms::Target Targetemp;
		Targetemp.SetPoseRobot(cvPoint((int)m_pOutput->myX,(int)m_pOutput->myY),m_pOutput->myTh);
		Targetemp.SetPoseTarget(cvPoint(4000,4000),30);
		Targetemp.SartUp();
		m_TargetVector.push_back(Targetemp);

	}
}
	cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[0],5,cvScalar(0,0,200),-1);
	cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[1],5,cvScalar(0,0,200),-1);
	cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[2],5,cvScalar(0,0,200),-1);
	cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[3],5,cvScalar(0,0,200),-1);
	cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[4],5,cvScalar(0,0,200),-1);
	cvLine(m_pSourceImage,m_Reference.m_TargetPoints[0],m_Reference.m_TargetPoints[2],cvScalar(0,0,200));
	cvLine(m_pSourceImage,m_Reference.m_TargetPoints[1],m_Reference.m_TargetPoints[3],cvScalar(0,0,200));

}

void TrackingTask::CreateWindow(std::string string )
{
	cvNamedWindow( string.c_str(), CV_WINDOW_AUTOSIZE );
}

void TrackingTask::DestroyWindow(std::string string )
{
	cvDestroyWindow( string.c_str());
}

void TrackingTask::SetReference(int val)
{
if(val==NEWTARGET)
{
	while(!m_bFindTargetPoints)
	{
		GetImage(FROMSERVER);
		FindTarget( );
		FindTargetPoints( );
		if(m_bFindTargetPoints)
		{
			m_Reference.m_TargetPoints[0].x=m_TargetPointsTemp.at(0).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			m_Reference.m_TargetPoints[1].x=m_TargetPointsTemp.at(1).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			m_Reference.m_TargetPoints[2].x=m_TargetPointsTemp.at(2).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			m_Reference.m_TargetPoints[3].x=m_TargetPointsTemp.at(3).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			m_Reference.m_TargetPoints[4].x=m_TargetPointsTemp.at(4).x+m_TargetBox.center.x-m_TargetBox.size.width/2;
			m_Reference.m_TargetPoints[0].y=m_TargetPointsTemp.at(0).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			m_Reference.m_TargetPoints[1].y=m_TargetPointsTemp.at(1).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			m_Reference.m_TargetPoints[2].y=m_TargetPointsTemp.at(2).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			m_Reference.m_TargetPoints[3].y=m_TargetPointsTemp.at(3).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			m_Reference.m_TargetPoints[4].y=m_TargetPointsTemp.at(4).y+m_TargetBox.center.y-m_TargetBox.size.height/2;
			m_TargetPointsTemp.clear();
			m_Segmentation.ClearTargetPoints();
			m_Reference.SartUp();
			printf("0:x:%d,y:%d  1:x:%d,y:%d  2:x:%d,y:%d  3:x:%d,y:%d  4:x:%d,y:%d\n",m_Reference.m_TargetPoints[0].x,m_Reference.m_TargetPoints[0].y,m_Reference.m_TargetPoints[1].x,m_Reference.m_TargetPoints[1].y,m_Reference.m_TargetPoints[2].x,m_Reference.m_TargetPoints[2].y,m_Reference.m_TargetPoints[3].x,m_Reference.m_TargetPoints[3].y,m_Reference.m_TargetPoints[4].x,m_Reference.m_TargetPoints[4].y);
			cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[0],5,cvScalar(0,0,200),-1);
			cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[1],5,cvScalar(0,0,200),-1);
			cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[2],5,cvScalar(0,0,200),-1);
			cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[3],5,cvScalar(0,0,200),-1);
			cvCircle(m_pSourceImage,m_Reference.m_TargetPoints[4],5,cvScalar(0,0,200),-1);
			cvLine(m_pSourceImage,m_Reference.m_TargetPoints[0],m_Reference.m_TargetPoints[2],cvScalar(0,0,200));
			cvLine(m_pSourceImage,m_Reference.m_TargetPoints[1],m_Reference.m_TargetPoints[3],cvScalar(0,0,200));

		}
		ShowDestinImage(5,std::string("result"));
		ShowSourceImage(5,std::string("source"));
		ReleaseDestinImage();
		ReleaseRoiImage();
		ReleaseSourceImage();
	}
}
else if(val==PREDEFTARGET1)
{
	GetImage(FROMSERVER);
	m_Reference.m_TargetPoints[0].x=139;
	m_Reference.m_TargetPoints[1].x=218;
	m_Reference.m_TargetPoints[2].x=220;
	m_Reference.m_TargetPoints[3].x=139;
	m_Reference.m_TargetPoints[4].x=175;

	m_Reference.m_TargetPoints[0].y=123;
	m_Reference.m_TargetPoints[1].y=123;
	m_Reference.m_TargetPoints[2].y=203;
	m_Reference.m_TargetPoints[3].y=203;
	m_Reference.m_TargetPoints[4].y=173;
	m_Reference.SartUp();


}
}

void TrackingTask::GetImage(int val)
{
	if(val==FROMSERVER)
	{
		m_pClient->requestOnce("sendVideo");
		ArUtil::sleep(150);
		m_pSourceImage=cvLoadImage("temp.jpg");
	}
	else if(val==FROMCAM)
	{
		m_pSourceImage=cvQueryFrame(m_pCapture);
	}
}

void TrackingTask::ClearBuffer( )
{
	while(m_iCountBuffer<5)
	{
		printf("Clear\n");
		GetImage(FROMSERVER);
		ArUtil::sleep(400);
		ShowSourceImage(5,std::string("source"));
		ReleaseSourceImage();
		m_iCountBuffer++;
	}
	m_iCountBuffer=0;
	m_TargetVector.clear();
	m_iCountTarget=0;
	m_bFillVect=false;
}

std::vector <algorithms::Target> TrackingTask::GetTargetVector()
{
	return m_TargetVector;
}

std::vector <algorithms::Target> TrackingTask::GetInterpoledVector()
{

	algorithms::Target Targetemp;

	Targetemp.m_TargetPoints[0].x=m_TargetVector.at(3).m_TargetPoints[0].x+(m_TargetVector.at(3).m_TargetPoints[0].x-m_TargetVector.at(2).m_TargetPoints[0].x);
	Targetemp.m_TargetPoints[1].x=m_TargetVector.at(3).m_TargetPoints[1].x+(m_TargetVector.at(3).m_TargetPoints[1].x-m_TargetVector.at(2).m_TargetPoints[1].x);
	Targetemp.m_TargetPoints[2].x=m_TargetVector.at(3).m_TargetPoints[2].x+(m_TargetVector.at(3).m_TargetPoints[2].x-m_TargetVector.at(2).m_TargetPoints[2].x);
	Targetemp.m_TargetPoints[3].x=m_TargetVector.at(3).m_TargetPoints[3].x+(m_TargetVector.at(3).m_TargetPoints[3].x-m_TargetVector.at(2).m_TargetPoints[3].x);
	Targetemp.m_TargetPoints[4].x=m_TargetVector.at(3).m_TargetPoints[4].x+(m_TargetVector.at(3).m_TargetPoints[4].x-m_TargetVector.at(2).m_TargetPoints[4].x);
	Targetemp.m_TargetPoints[0].y=m_TargetVector.at(3).m_TargetPoints[0].y+(m_TargetVector.at(3).m_TargetPoints[0].y-m_TargetVector.at(2).m_TargetPoints[0].y);
	Targetemp.m_TargetPoints[1].y=m_TargetVector.at(3).m_TargetPoints[1].y+(m_TargetVector.at(3).m_TargetPoints[1].y-m_TargetVector.at(2).m_TargetPoints[1].y);
	Targetemp.m_TargetPoints[2].y=m_TargetVector.at(3).m_TargetPoints[2].y+(m_TargetVector.at(3).m_TargetPoints[2].y-m_TargetVector.at(2).m_TargetPoints[2].y);
	Targetemp.m_TargetPoints[3].y=m_TargetVector.at(3).m_TargetPoints[3].y+(m_TargetVector.at(3).m_TargetPoints[3].y-m_TargetVector.at(2).m_TargetPoints[3].y);
	Targetemp.m_TargetPoints[4].y=m_TargetVector.at(3).m_TargetPoints[4].y+(m_TargetVector.at(3).m_TargetPoints[4].y-m_TargetVector.at(2).m_TargetPoints[4].y);
	Targetemp.m_TargetPointsRef[0].x=m_Reference.m_TargetPoints[0].x;
	Targetemp.m_TargetPointsRef[1].x=m_Reference.m_TargetPoints[1].x;
	Targetemp.m_TargetPointsRef[2].x=m_Reference.m_TargetPoints[2].x;
	Targetemp.m_TargetPointsRef[3].x=m_Reference.m_TargetPoints[3].x;
	Targetemp.m_TargetPointsRef[4].x=m_Reference.m_TargetPoints[4].x;
	Targetemp.m_TargetPointsRef[0].y=m_Reference.m_TargetPoints[0].y;
	Targetemp.m_TargetPointsRef[1].y=m_Reference.m_TargetPoints[1].y;
	Targetemp.m_TargetPointsRef[2].y=m_Reference.m_TargetPoints[2].y;
	Targetemp.m_TargetPointsRef[3].y=m_Reference.m_TargetPoints[3].y;
	Targetemp.m_TargetPointsRef[4].y=m_Reference.m_TargetPoints[4].y;
	m_TargetVector.erase(m_TargetVector.begin());
	Targetemp.SetPoseRobot(cvPoint((int)m_pOutput->myX,(int)m_pOutput->myY),m_pOutput->myTh);
	Targetemp.SartUp();
	m_TargetVector.push_back(Targetemp);

	printf("1:%d\n",m_TargetVector.at(0).GetDiffCenterX());
	printf("2:%d\n",m_TargetVector.at(1).GetDiffCenterX());
	printf("3:%d\n",m_TargetVector.at(2).GetDiffCenterX());
	printf("4:%d\n",m_TargetVector.back().GetDiffCenterX());
	printf("size:%d\n",m_TargetVector.size());



	return m_TargetVector;
}

//!Protected Methods

//!Private Methods

void TrackingTask::SetRoi(CvPoint point1,CvPoint point2)
{
	if(point1.x<0)
	{
		point1.x=0;
	}
	if(point1.x>m_pSourceImage->width-1)
	{
		point1.x=m_pSourceImage->width-1;
	}
	if(point1.y<0)
	{
		point1.y=0;
	}
	if(point1.y>m_pSourceImage->height-1)
	{
		point1.y=m_pSourceImage->height-1;
	}
	if(point2.x<0)
	{
		point2.x=0;
	}
	if(point2.x>m_pSourceImage->width-1)
	{
		point2.x=m_pSourceImage->width-1;
	}
	if(point2.y<0)
	{
		point2.y=0;
	}
	if(point2.y>m_pSourceImage->height-1)
	{
		point2.y=m_pSourceImage->height-1;
	}
	m_pRoi=cvCreateImage(cvSize(point2.x-point1.x,point2.y-point1.y),m_pSourceImage->depth,m_pSourceImage->nChannels);

	uchar* pRoi=(uchar*)m_pRoi->imageData;
	uchar* pSource=(uchar*)m_pSourceImage->imageData;

	for(int y=0,j=point1.y;point2.y>j;j++,y++)
	{
		for(int x=0,i=3*point1.x;3*(point2.x)>i;i++,x++)
		{
			pRoi[x+y*m_pRoi->widthStep]=pSource[i+j*m_pSourceImage->widthStep];
			pRoi[x+1+y*m_pRoi->widthStep]=pSource[i+1+j*m_pSourceImage->widthStep];
			pRoi[x+2+y*m_pRoi->widthStep]=pSource[i+2+j*m_pSourceImage->widthStep];
		}
	}


}

/**
 * Starts all the attributes of
 * the TrackingTask
 */

void TrackingTask::initAttributes ( )
{
	m_pCapture=cvCreateCameraCapture(0);
	m_bFindTarget=false;
	m_bFindTargetPoints=false;
	m_iCountTarget=0;
	m_iCountBuffer=0;
	m_iCountMaxTarget=3;
	m_bFillVect=false;
}

}
